#include "Digital_Filters.h"

#pragma DATA_SEG MY_ZEROPAGE
volatile uUN32 uUN32tempo;
volatile uint8 u8tempo;

volatile uUN16 uUN16coefA[3];
volatile uUN16 uUN16coefB[3];
volatile uUN16 uUN16gain;

volatile uUN16 uUN16Xy1,uUN16Xy2;
volatile uint8 u8Xx1 = 0,u8Xx2 = 0;

volatile uUN16 uUN16Yy1,uUN16Yy2;
volatile uint8 u8Yx1 = 0,u8Yx2 = 0;

volatile uUN16 uUN16Zy1,uUN16Zy2;
volatile uint8 u8Zx1 = 0,u8Zx2 = 0;

uUN16 uUN16multres;
uint16 u16sum;
uint8 u8result;
int8 i8tempovar;

#pragma MESSAGE DISABLE C4001

void vfnFilterInit (void)
{ 
    // Code Generated using MatLab
    //Filter design for Zeppelin project
    //This filter is aplied in the elimination of noise of the accelerometer signal on controller

    //    fs = +6.25000000e+001 Hz
    //    A = [ +1.00000000e+000, -1.82047994e+000, +8.35724077e-001 ]
    //    B = [ +1.00000000e+000, -1.53404451e+000, +1.00000000e+000 ]
    //    Gain = +3.27158649e-002
    /*
    const uint16 u16A[] = [ 0x4000, 0x7483, 0x357D ];   //SQ14
    const uint16 u16B[] = [ 0x4000, 0x622E, 0x4000 ];   //SQ14
    const uint16 u16Gain = 0x2180;   //SQ18
    */
    
    uUN16gain.s = 0x2180;	// Positive
	
    uUN16coefA[0].s = 0x4000;    // Positive
    uUN16coefA[1].s = 0x7483;    // Negative
    uUN16coefA[2].s = 0x357D;    // Positive

    uUN16coefB[0].s = 0x4000;    // Positive
    uUN16coefB[1].s = 0x622E;    // Negative
    uUN16coefB[2].s = 0x4000;    // Positive

    // Initialize variables used in the filtering process for last values

    uUN16Xy1.s = 0;
    uUN16Xy2.s = 0;
    	
    uUN16Yy1.s = 0;
    uUN16Yy2.s = 0;
    	
    uUN16Zy1.s = 0;
    uUN16Zy2.s = 0;
    	
}


uint8 u8FilterADCX (uint8 u8input)
{
	mMul16x8to32Q14(uUN16coefB[0], u8input, uUN16multres.s);	
	u16sum = (uint16)uUN16multres.s;
		
	mMul16x8to32Q14(uUN16coefB[2], u8Xx2, uUN16multres.s);	
	u16sum += (uint16)uUN16multres.s;
	
	mMul16x16to32Q14(uUN16coefA[1], uUN16Xy1, uUN16multres.s);	
	u16sum += (uint16)uUN16multres.s;
		
	mMul16x16to32Q14(uUN16coefA[2], uUN16Xy2, uUN16multres.s);	
	u16sum -= (uint16)uUN16multres.s;
	
	mMul16x8to32Q14(uUN16coefB[1], u8Xx1, uUN16multres.s);	
	u16sum -= (uint16)uUN16multres.s;
	
	uUN16Xy2.s = uUN16Xy1.s;
	u8Xx2 = u8Xx1;
	uUN16Xy1.s = u16sum;
	u8Xx1 = u8input;
	
	uUN16multres.s = u16sum;
	
	mMul16x16to32Q18(uUN16gain, uUN16multres, u8result);
	
	if(u8result > ADC_8_BIT_MAX_AFTER_FILTER) 
	{
	    return ADC_8_BIT_MAX_AFTER_FILTER;
	}
	else if(u8result < ADC_8_BIT_MIN_AFTER_FILTER) 
	{
	   return ADC_8_BIT_MIN_AFTER_FILTER;
	}
	else
	{
	    return u8result;
	}
}

uint8 u8FilterADCY (uint8 u8input)
{
	mMul16x8to32Q14(uUN16coefB[0], u8input, uUN16multres.s);	
	u16sum = (uint16)uUN16multres.s;
		
	mMul16x8to32Q14(uUN16coefB[2], u8Yx2, uUN16multres.s);	
	u16sum += (uint16)uUN16multres.s;
	
	mMul16x16to32Q14(uUN16coefA[1], uUN16Yy1, uUN16multres.s);	
	u16sum += (uint16)uUN16multres.s;
		
	mMul16x16to32Q14(uUN16coefA[2], uUN16Yy2, uUN16multres.s);	
	u16sum -= (uint16)uUN16multres.s;      
	
	mMul16x8to32Q14(uUN16coefB[1], u8Yx1, uUN16multres.s);	
	u16sum -= (uint16)uUN16multres.s;
	
	uUN16Yy2.s = uUN16Yy1.s;
	u8Yx2 = u8Yx1;
	uUN16Yy1.s = u16sum;
	u8Yx1 = u8input;
	
	uUN16multres.s = u16sum;
	
	mMul16x16to32Q18(uUN16gain, uUN16multres, u8result);
	
	if(u8result > ADC_8_BIT_MAX_AFTER_FILTER) 
	{
	    return ADC_8_BIT_MAX_AFTER_FILTER;
	}
	else if(u8result < ADC_8_BIT_MIN_AFTER_FILTER) 
	{
	   return ADC_8_BIT_MIN_AFTER_FILTER;
	}
	else
	{
	    return u8result;
	}
}

uint8 u8FilterADCZ (uint8 u8input)
{
	mMul16x8to32Q14(uUN16coefB[0], u8input, uUN16multres.s);	
	u16sum = (uint16)uUN16multres.s;
		
	mMul16x8to32Q14(uUN16coefB[2], u8Zx2, uUN16multres.s);	
	u16sum += (uint16)uUN16multres.s;
	
	mMul16x16to32Q14(uUN16coefA[1], uUN16Zy1, uUN16multres.s);	
	u16sum += (uint16)uUN16multres.s;
		
	mMul16x16to32Q14(uUN16coefA[2], uUN16Zy2, uUN16multres.s);	
	u16sum -= (uint16)uUN16multres.s;
	
	mMul16x8to32Q14(uUN16coefB[1], u8Zx1, uUN16multres.s);	
	u16sum -= (uint16)uUN16multres.s;
	
	uUN16Zy2.s = uUN16Zy1.s;
	u8Zx2 = u8Zx1;
	uUN16Zy1.s = u16sum;
	u8Zx1 = u8input;
	
	uUN16multres.s = u16sum;
	
	mMul16x16to32Q18(uUN16gain, uUN16multres, u8result);
	
	if(u8result > ADC_8_BIT_MAX_AFTER_FILTER) 
	{
	    return ADC_8_BIT_MAX_AFTER_FILTER;
	}
	else if(u8result < ADC_8_BIT_MIN_AFTER_FILTER) 
	{
	   return ADC_8_BIT_MIN_AFTER_FILTER;
	}
	else
	{
	    return u8result;
	}
}
